#include "MephistoCAM.h"

void set_FLOAT_SPI_PS_A3( uint32_t *gpio_reg, uint32_t A3 ){
    
    if( A3 == 1 )
        setbit( *gpio_reg, BIT_FLOAT_SPI_PS_A3 );

    if( A3 == 0 )
        clrbit( *gpio_reg, BIT_FLOAT_SPI_PS_A3 );
}

void set_FLOAT_SPI_PS_A2( uint32_t *gpio_reg, uint32_t A2 ){

    if( A2 == 1 )
        setbit( *gpio_reg, BIT_FLOAT_SPI_PS_A2 );

    if( A2 == 0 )
        clrbit( *gpio_reg, BIT_FLOAT_SPI_PS_A2 );
}

void set_FLOAT_SPI_PS_A1( uint32_t *gpio_reg, uint32_t A1 ){

    if( A1 == 1 )
        setbit( *gpio_reg, BIT_FLOAT_SPI_PS_A1 );

    if( A1 == 0 )
        clrbit( *gpio_reg, BIT_FLOAT_SPI_PS_A1 );
}

void set_FLOAT_SPI_PS_A0( uint32_t *gpio_reg, uint32_t A0 ){

    if( A0 == 1 )
        setbit( *gpio_reg, BIT_FLOAT_SPI_PS_A0 );

    if( A0 == 0 )
        clrbit( *gpio_reg, BIT_FLOAT_SPI_PS_A0 );
}

void set_FLOAT_SPI_PS_SET_OS_CH( uint32_t *gpio_reg, uint32_t os_ch ){

    uint32_t A3 = 0;
    uint32_t A2 = 0;
    uint32_t A1 = 0;
    uint32_t A0 = 0;

    A3 = getbit( os_ch, 3 );
    A2 = getbit( os_ch, 2 );
    A1 = getbit( os_ch, 1 );
    A0 = getbit( os_ch, 0 );

//  debug
    // printf(" os_ch = %d\n", os_ch);
    // print_as_bin(os_ch);
    // printf("A3 = %u\n", A3);
    // printf("A2 = %u\n", A2);
    // printf("A1 = %u\n", A1);
    // printf("A0 = %u\n", A0);

    printf(", {A3,A2,A1,A0} = %d%d%d%d ", A3,A2,A1,A0);

    set_FLOAT_SPI_PS_A3( gpio_reg, A3 );
    set_FLOAT_SPI_PS_A2( gpio_reg, A2 );
    set_FLOAT_SPI_PS_A1( gpio_reg, A1 );
    set_FLOAT_SPI_PS_A0( gpio_reg, A0 );
}

/*

*/
void FLOAT_SPI_PS_SCLK( uint32_t *gpio_fp_ctrl ){
    clrbit(*gpio_fp_ctrl, BIT_FLOAT_SPI_PS_SCLK);
    // usleep(4);
    setbit(*gpio_fp_ctrl, BIT_FLOAT_SPI_PS_SCLK);
    // usleep(4);
}

void FLOAT_SPI_PS_RST( uint32_t *gpio_fp_ctrl ){
    clrbit(*gpio_fp_ctrl, BIT_FLOAT_SPI_PS_RST);
    // usleep(5);
    setbit(*gpio_fp_ctrl, BIT_FLOAT_SPI_PS_RST);
    // usleep(5);
    clrbit(*gpio_fp_ctrl, BIT_FLOAT_SPI_PS_RST);
    // usleep(5);
}

void FLOAT_SPI_PS_SET_ADDR( uint32_t *gpio_fp_ctrl, uint16_t addr ){
    uint32_t addr_tmp = *gpio_fp_ctrl & 0xffff0000;
    addr_tmp = addr_tmp | addr;
    *gpio_fp_ctrl = addr_tmp;
}

void FLOAT_SPI_PS_SET_DATA( uint32_t *gpio_fp_data, uint32_t data ){
    *gpio_fp_data = data;
}

void Config_Float_Regs_PL(  uint32_t *gpio_fp_ctrl,
                            uint32_t *gpio_fp_data,
                            uint16_t data_size,
                            uint32_t *float_regs ){

    uint16_t i;

    //  0) reset
    FLOAT_SPI_PS_RST( gpio_fp_ctrl );

    //  1) start sending data
    for( i=0; i<data_size; i++ ){
    // for( i=0; i<100; i++ ){
        
        // set addr
        FLOAT_SPI_PS_SET_ADDR( gpio_fp_ctrl, i );

        // set data
        FLOAT_SPI_PS_SET_DATA( gpio_fp_data, float_regs[i] );

        // pulse a sclk
        FLOAT_SPI_PS_SCLK( gpio_fp_ctrl );
    }
}

void Config_DCDS_Weights(   uint32_t *gpio_fp_ctrl,
                            uint32_t *gpio_fp_data,
                            uint16_t data_size,
                            uint16_t os_ch,
                            uint32_t *float_regs ){

    printf("> writing dcds weights for OS channel: %2d ", os_ch+1);
    set_FLOAT_SPI_PS_SET_OS_CH( gpio_fp_ctrl, os_ch );

    Config_Float_Regs_PL(   gpio_fp_ctrl,
                            gpio_fp_data,
                            data_size,
                            float_regs );

    printf(" ...... done\n");
}
